/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2022 STMicroelectronics.
  * All rights reserved.</center></h2>
  *
  * This software component is licensed by ST under BSD 3-Clause license,
  * the "License"; You may not use this file except in compliance with the
  * License. You may obtain a copy of the License at:
  *                        opensource.org/licenses/BSD-3-Clause
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "dma.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "includes.h"
#include "remote control.h"
#include "math.h"
#define a 140
#define b 105
#define kw 0.3
#define k 10
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
float servo_luoqiukou;
float servo_fandou;
float mocalun;
float speed1,speed2;
float servo_yuntai_yaw;
float servo_yuntai_pitch;
float vx,vy,w,Vw1,Vw2,Vw3,Vw4;
float dj[4];
float V1,V2,V3,V4;
float kv1;
int pijingunlunkaiguan;
int mocalunkaiguan;
/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

	
  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_USART3_UART_Init();
  MX_TIM1_Init();
  MX_TIM2_Init();
  MX_TIM3_Init();
  MX_TIM4_Init();
  MX_TIM8_Init();
  /* USER CODE BEGIN 2 */
  //ң������ʼ��
	 Remote_Control_Init(&huart3);
	 __HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,500);
	 __HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,500);
	 __HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_4,0);
	 __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_1,0);
	 __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_2,0);
	 __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_3,0);
	 __HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_3,1450);
	 __HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_4,1425);
	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_2);
	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_3);
	HAL_TIM_PWM_Start(&htim2 ,TIM_CHANNEL_4);
	HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_1);
	HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_2);
	HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_3);
	HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_3);
	HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_4);
	servo_luoqiukou=500;
	servo_fandou=600;
	mocalun=0;
	speed1=0;
	speed2=0;
	servo_yuntai_yaw=1450;
	servo_yuntai_pitch=1350;
	pijingunlunkaiguan =0;
	mocalunkaiguan=0;
	__HAL_TIM_SetCompare(&htim4 ,TIM_CHANNEL_1 ,0);
 __HAL_TIM_SetCompare(&htim4 ,TIM_CHANNEL_2 ,0);
 HAL_TIM_PWM_Start (&htim4 ,TIM_CHANNEL_1);
 HAL_TIM_PWM_Start (&htim4 ,TIM_CHANNEL_2);
 __HAL_TIM_SetCompare(&htim4 ,TIM_CHANNEL_3 ,0);
 __HAL_TIM_SetCompare(&htim4 ,TIM_CHANNEL_4 ,0);
 HAL_TIM_PWM_Start (&htim4 ,TIM_CHANNEL_3);
 HAL_TIM_PWM_Start (&htim4 ,TIM_CHANNEL_4);
  __HAL_TIM_SetCompare(&htim8 ,TIM_CHANNEL_1 ,0);
 __HAL_TIM_SetCompare(&htim8 ,TIM_CHANNEL_2 ,0);
 HAL_TIM_PWM_Start (&htim8 ,TIM_CHANNEL_1);
 HAL_TIM_PWM_Start (&htim8 ,TIM_CHANNEL_2);
 __HAL_TIM_SetCompare(&htim8 ,TIM_CHANNEL_3 ,0);
 __HAL_TIM_SetCompare(&htim8 ,TIM_CHANNEL_4 ,0);
 HAL_TIM_PWM_Start (&htim8 ,TIM_CHANNEL_3);
 HAL_TIM_PWM_Start (&htim8 ,TIM_CHANNEL_4);
  /* USER CODE END 2 */
  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {
		__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_4,mocalun);
		__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_2,servo_luoqiukou);
		__HAL_TIM_SetCompare(&htim2,TIM_CHANNEL_3,servo_fandou);
		__HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_1,speed1);
		__HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_2,speed1);
	  __HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_3,speed2);
		__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_3,servo_yuntai_yaw);
		__HAL_TIM_SetCompare(&htim3,TIM_CHANNEL_4,servo_yuntai_pitch);
		kv1 = 130*(1+((float)remote_control .mouse .z/10000));
		if(kv1<50)
		{
		kv1=50;
		}
		if(remote_control .switch_left==3) 
			{				
		servo_luoqiukou=2500;
			}
		if(remote_control .switch_left==2|remote_control .switch_left ==1) {servo_luoqiukou=500;}
		if((remote_control .switch_right==1)&(servo_fandou<1800)&pijingunlunkaiguan >=500)
		{
		servo_fandou ++;
		speed1 = 3000;
		}
		if((remote_control .switch_right==2)&(servo_fandou>600)&pijingunlunkaiguan >=500)
		{
		servo_fandou --;
		speed1 = 1000;
		}
		if(remote_control .switch_right==3&pijingunlunkaiguan >=500)  speed1=1000;
		if(remote_control .key_code  ==6) {servo_yuntai_yaw=1450;servo_yuntai_pitch=1350;} 
		if(remote_control .key_code  ==4) {servo_yuntai_yaw=500;servo_yuntai_pitch=1350;}
		if(remote_control .key_code  ==2) {servo_yuntai_yaw=2500;servo_yuntai_pitch=1350;}
		if(remote_control .key_code ==8&servo_yuntai_yaw>=700)
		{
		servo_yuntai_yaw -=3;
		}
		if(remote_control .key_code ==1&servo_yuntai_yaw<=2300)
		{
		servo_yuntai_yaw +=3;
		}
		if(remote_control .ch4>=-660&remote_control .ch4<-300 &servo_yuntai_pitch<1600)
		{
		servo_yuntai_pitch +=0.5;
		}
		if(remote_control .ch4 <=660&remote_control .ch4 >300&servo_yuntai_pitch>1200)
		{
		servo_yuntai_pitch -=0.5;
		}
		if(remote_control .ch2>400|remote_control .ch2<-400)
		{vy = (float)remote_control .ch2/(660)*kv1;}
		else vy=0;
		if(remote_control .ch1>400|remote_control .ch1 <-400)
		{vx = (float)(remote_control .ch1)/(-660)*kv1;}
		else vx=0;
		if(remote_control .ch3>600|remote_control .ch3 <-600)
		{w = (float)(remote_control .ch3)/(-660)*kw;}
		else w=0;
  	Vw1=vy-vx+w*(a+b);
	Vw2=vy+vx-w*(a+b);
	Vw3=vy-vx-w*(a+b);
	Vw4=vy+vx+w*(a+b);
	if(Vw1>0) dj[0]=1, V1=Vw1*k;
	if(Vw1<0) dj[0]=0, V1=-Vw1*k;
	   if(Vw1==0) V1=0;
	if(Vw2>0) dj[1]=1, V2=Vw2*k;
	if(Vw2<0) dj[1]=0, V2=-Vw2*k;
		if(Vw2==0) V2=0;
	if(Vw3>0) dj[2]=1, V3=Vw3*k;
	if(Vw3<0) dj[2]=0, V3=-Vw3*k;
		if(Vw3==0) V3=0;
	if(Vw4>0) dj[3]=1, V4=Vw4*k;
	if(Vw4<0) dj[3]=0, V4=-Vw4*k;
	if(Vw4==0) V4=0;
		if(dj[0]==1)
		{
			__HAL_TIM_SetCompare(&htim4,TIM_CHANNEL_1,V1);
			__HAL_TIM_SetCompare(&htim4,TIM_CHANNEL_2,0);
		}
		if(dj[0]==0)
		{
			__HAL_TIM_SetCompare(&htim4,TIM_CHANNEL_1,0);
			__HAL_TIM_SetCompare(&htim4,TIM_CHANNEL_2,V1);
		}
		if(dj[1]==1)
		{
			__HAL_TIM_SetCompare(&htim4,TIM_CHANNEL_3,V2);
			__HAL_TIM_SetCompare(&htim4,TIM_CHANNEL_4,0);
		}
		if(dj[1]==0)
		{
			__HAL_TIM_SetCompare(&htim4,TIM_CHANNEL_3,0);
			__HAL_TIM_SetCompare(&htim4,TIM_CHANNEL_4,V2);
		}
		if(dj[2]==1)
		{
			__HAL_TIM_SetCompare(&htim8,TIM_CHANNEL_1,V3);
			__HAL_TIM_SetCompare(&htim8,TIM_CHANNEL_2,0);
		}
		if(dj[2]==0)
		{
			__HAL_TIM_SetCompare(&htim8,TIM_CHANNEL_1,0);
			__HAL_TIM_SetCompare(&htim8,TIM_CHANNEL_2,V3);
		}
		if(dj[3]==1)
		{
			__HAL_TIM_SetCompare(&htim8,TIM_CHANNEL_3,V4);
			__HAL_TIM_SetCompare(&htim8,TIM_CHANNEL_4,0);
		}
		if(dj[3]==0)
		{
			__HAL_TIM_SetCompare(&htim8,TIM_CHANNEL_3,0);
			__HAL_TIM_SetCompare(&htim8,TIM_CHANNEL_4,V4);
		}
		if(remote_control .mouse .press_right ==1)
		{
			pijingunlunkaiguan ++;
		}
		if(remote_control .mouse .press_left ==1)
		{
			mocalunkaiguan ++;
		}
		if(pijingunlunkaiguan>=500)
		{
		speed1 =1000;
		speed2 =800;
		}
		if(pijingunlunkaiguan<500)
		{
		speed1=0;
		speed2=0;
		}
		if(pijingunlunkaiguan <0)
		{
		pijingunlunkaiguan =0;
		}
		if(pijingunlunkaiguan >1000)
		{
		pijingunlunkaiguan =0;
		}
		if(mocalunkaiguan >=500)
		{
			if(remote_control.switch_left ==1)
			{mocalun = 15000;}
			if(remote_control.switch_left ==2)
			{mocalun = 10000;}
		}
		if(mocalunkaiguan <500)
		{
		mocalun = 0;
		}
		if(mocalunkaiguan <0)
		{
			mocalunkaiguan = 0;
		}
		if(mocalunkaiguan>1000)
		{
		mocalunkaiguan = 0;
		}
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
		HAL_Delay(1);
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }
  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */

  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
